#include "MiniBSplineCurve.h"

using namespace XYF;

#define MIN(a, b) ((a) > (b) ? (b) : (a))
#define MAX(a, b) ((a) < (b) ? (b) : (a))

MiniBSplineCurve::MiniBSplineCurve(const std::vector<Vector3f> &cpts,
                                   const std::vector<Real> &knots,
                                   const int degree,
                                   const bool periodic)
    : m_cpts(cpts), m_knots(knots), m_degree(degree), m_periodic(periodic)
{
}

MiniBSplineCurve::MiniBSplineCurve(const int N, const int degree, const bool periodic)
    : m_cpts(N), m_degree(degree), m_periodic(periodic)
{
//    if (periodic)
//    {
//        // 周期情形需要补充重合控制点
//        for (int i = 0; i < degree; i++)
//            m_cpts.push_back(cpts[i]);

//        int n = m_cpts.size() - 1;
//        int d = degree;

//        for (int i = 0; i < n + d + 2; i++)
//            m_knots.push_back(1.0 * (i - d) / (n + 1 - d));
//    }
//    else
//    {
        int n = m_cpts.size() - 1;
        int d = degree;

        for (int i = 0; i < d + 1; i++)
            m_knots.push_back(0);
        for (int i = d + 1; i < n + 1; i++)
            m_knots.push_back(1.0 * (i - d) / (n + 1 - d));
        for (int i = n + 1; i < n + d + 2; i++)
            m_knots.push_back(1);
//    }
}

int MiniBSplineCurve::GetType()
{
    return MiniObject::B_spline_curve;
}

int MiniBSplineCurve::GetPointsNum() const
{
    return m_cpts.size();
}

const MiniArray<Vector3f> &MiniBSplineCurve::GetPoints() const
{
    return m_cpts;
}

MiniArray<Vector3f> &MiniBSplineCurve::GetPoints()
{
    return m_cpts;
}

int MiniBSplineCurve::GetDegree() const
{
    return m_degree;
}

bool MiniBSplineCurve::GetPeriodic() const
{
    return m_periodic;
}

MiniArray<Real> MiniBSplineCurve::GetKnots() const
{
    return m_knots;
}

MiniArray<Real> &MiniBSplineCurve::GetKnots()
{
    return m_knots;
}

Real MiniBSplineCurve::GetBegin() const
{
    return m_knots[m_degree];
}

Real MiniBSplineCurve::GetEnd() const
{
    return m_knots[m_knots.size() - m_degree - 1];
}

Vector3f MiniBSplineCurve::CurvePoint(Real u) const
{
    // 先计算出所有非零基函数，然后与对应的控制点相乘求和
    int span = FindSpan(u, m_degree, m_knots);
    MiniArray<Real> N = BasisFuncs(span, u, m_degree, m_knots);

    Vector3f p;
    for (int i = 0; i < int(N.size()); i++)
        p = p + m_cpts[i + span - m_degree] * N[i];
    return p;
}

MiniArray2D<Vector3f> MiniBSplineCurve::CurveDerivCpts(int d, int r1, int r2) const
{
    assert(d <= m_degree);
    assert(r1 >= 0);
    assert(r2 < int(m_cpts.size()));

    // 0 阶导控制点（初始控制点）
    int r = r2 - r1;
    MiniArray2D<Vector3f> PK(d + 1, MiniArray<Vector3f>(r + 1));
    for (int i = 0; i < r + 1; i++)
        PK[0][i] = m_cpts[r1 + i];

    int p = GetDegree();
    for (int k = 1; k < d + 1; k++)
    {
        int tmp = p - k + 1;
        // P_i^k = (P_{i+1}^{k-1}-P_i^{k-1}) / (u_{i+p+1}-u_{i+k}) * (p-k+1)
        for (int i = 0; i < r - k + 1; i++)
            PK[k][i] = (PK[k - 1][i + 1] - PK[k - 1][i]) / (m_knots[r1 + i + p + 1] - m_knots[r1 + i + k]) * tmp;
    }
    return PK;
}

MiniArray<Vector3f> MiniBSplineCurve::CurveDerivPoint(int d, Real u) const
{
    int p = GetDegree();
    MiniArray<Vector3f> CK(d + 1);

    // 导数不超过次数，超过的部分归零
    int du = MIN(p, d);
    int span = FindSpan(u, p, m_knots);

    // 获得从 0 次开始的所有非零基函数
    MiniArray2D<Real> N = AllBasisFuncs(span, u, p, m_knots);
    MiniArray2D<Vector3f> PK = CurveDerivCpts(du, span - p, span);

    // N[j][r] = N_{r,j}
    for (int k = 0; k < du + 1; k++)
        for (int j = 0; j < p - k + 1; j++)
            CK[k] = CK[k] + PK[k][j] * N[p - k][j];
    return CK;
}

void MiniBSplineCurve::CurveKnotIns(Real u, int r)
{
    int p = GetDegree();
    int n = GetPointsNum() - 1;
    int m = n + p + 1;
    int k = FindSpan(u, p, m_knots);        // 找到插入位置
    int s = FindSpanMult(k, u, p, m_knots); // 获得插入节点之前的重数
    MiniArray<Vector3f> &Q = m_cpts;
    MiniArray<Real> &U = m_knots;

    // 保存用于迭代的控制点 k-p 到 k-s
    MiniArray<Vector3f> R(p + 1);
    for (int i = 0; i < p - s + 1; i++)
        R[i] = Q[k - p + i];

    // 前面 k + 1 个控制点不变
    Q.resize(n + r + 1);
    // 后面不变的控制点，向后移动 r 位（倒过来移动）
    for (int i = n; i >= k - s; i--)
        Q[i + r] = Q[i];

    // 循环插入
    for (int j = 1; j < r + 1; j++)
    {
        int L = 0; // 用于 R 的索引

        // k - p + r <= i <= k - s
        for (int i = k - p + j; i < k - s + 1; i++, L++)
        {
            Real alpha = (u - U[i]) / (U[i + p - j + 1] - U[i]);
            R[L] = R[L + 1] * alpha + R[L] * (1 - alpha);
        }

        // 顶部和底部两个控制点不变
        Q[k - p + j] = R[0];
        Q[k - s + r - j] = R[p - s - j];
    }

    // 存储中间的迭代结果
    int L = k - p + r;
    for (int i = L + 1; i < k - s; i++)
        Q[i] = R[i - L];

    // 最后修改节点
    U.resize(m + r + 1);
    for (int i = m + r; i > k + r; i--)
        U[i] = U[i - r];
    for (int i = 1; i < r + 1; i++)
        U[k + i] = u;
}

void MiniBSplineCurve::CurveRefineKnot(const MiniArray<Real> &X)
{
    int n = GetPointsNum() - 1;
    int p = GetDegree();
    int m = n + p + 1;
    int r = X.size() - 1;
    MiniArray<Vector3f> &Q = m_cpts;
    MiniArray<Real> &U = m_knots;

    // 获得要插入的区间范围
    Real a = FindSpan(X[0], p, U);
    Real b = FindSpan(X[r], p, U) + 1;

    // 前面 a + 1 个控制点不变
    Q.resize(n + r + 2);
    // 后面的控制点平移 r+1
    for (int j = n; j >= b - 1; j--)
        Q[j + r + 1] = Q[j];

    // 前面 a + 1 个节点不变，后面节点向后平移 r+1
    U.resize(m + r + 2);
    for (int j = m; j >= b + p; j--)
        U[j + r + 1] = U[j];

    // 从最后一个要变化的节点开始向前循环迭代
    int i = b + p - 1;
    int k = b + p + r;
    for (int j = r; j >= 0; j--)
    {
        // 插入一个节点，更新控制点
        while (X[j] <= U[i] && i > a)
        {
            Q[k - p - 1] = Q[i - p - 1];
            U[k] = U[i];

            k--;
            i--;
        }
        Q[k - p - 1] = Q[k - p];
        for (int l = 1; l < p + 1; l++)
        {
            int ind = k - p + l;
            Real alfa = U[k + l] - X[j];
            if (fabs(alfa) == 0)
                Q[ind - 1] = Q[ind];
            else
            {
                alfa /= U[k + l] - U[i - p + l];
                Q[ind - 1] = Q[ind - 1] * alfa + Q[ind] * (1 - alfa);
            }
        }
        U[k] = X[j];
        k--;
    }
}

Real MiniBSplineCurve::NearPointProjection(Vector3f P, Real tol)
{
    int p = GetDegree();
    int n = m_cpts.size() - 1;
    int m = n + p + 1;

    // 对每个有效节点区间等分寻找最近点
    Real u0 = GetBegin();
    Real near = distance(P, CurvePoint(u0));
    for (int i = p; i < m - p; i++)
    {
        Real u = m_knots[i];
        Vector3f Q = CurvePoint(u);
        Real d = distance(P, Q);
        if (d < near)
        {
            near = d;
            u0 = u;
        }
    }

    // 应用牛顿迭代法
    Real dp = tol * 10;
    while (near > tol && dp > tol)
    {
        MiniArray<Vector3f> C = CurveDerivPoint(2, u0);
        Real u1 = u0 - dot(C[1], C[0] - P) / (dot(C[2], C[0] - P) + pow(length(C[1]), 2));
        near = distance(P, CurvePoint(u1));
        dp = length(C[1]) * fabs(u1 - u0);
        u0 = u1;
    }
    return u0;
}

Real MiniBSplineCurve::OrthoPointProjection(Vector3f P, Real tol)
{
    // 使用切线进行投影
    Real u0 = (GetBegin() + GetEnd()) / 2;
    Real dp = tol * 10;
    while (dp > tol)
    {
        MiniArray<Vector3f> C = CurveDerivPoint(2, u0);
        Vector3f T = normalize(C[1]);

        // 计算 P 到切线上的投影点，得到增量一阶方法
        Vector3f Q = C[0] + T * dot(P - C[0], T);
        Real dt = dot(C[1], Q - C[0]) / dot(C[1], C[1]);
        u0 += dt;

        dp = length(C[1]) * fabs(dt);
    }
    return u0;
}
